﻿/* Copyright 2007-2008 dnAnalytics Project.
 *
 * Contributors to this file:
 * Marcus Cuda - http://marcus.cuda.net
 *
 * This file is part of dnAnalytics.  dnAnalytics is licensed under the 
 * Microsoft Public License. See License.txt for a complete copy of the
 * license.
 */
namespace dnAnalytics.LinearAlgebra.Decomposition
{
    //port of the public domain Jampack QR
    internal class UserHouseholder : AbstractHouseholder
    {
        public UserHouseholder(Matrix matrix) : base(matrix)
        {
        }

        protected override void DoCompute()
        {
            mQ = new DenseMatrix(mRows);

            for (int i = 0; i < mRows; i++)
            {
                mQ.ValueAt(i, i, 1.0);
            }
            int minmn = System.Math.Min(mRows, mColumns);

            double[][] u = new double[minmn][];
            for (int i = 0; i < minmn; i++)
            {
                u[i] = GenerateColumn(mR, i, mRows - 1, i);
                UA(u[i], mR, i, mRows - 1, i + 1, mColumns - 1);
            }

            for (int i = minmn - 1; i >= 0; i--)
            {
                UA(u[i], mQ, i, mRows - 1, i, mRows - 1);
            }
        }

        protected override void DoSolve(Matrix b, Matrix result)
        {
            // Compute Y = transpose(Q)*B
            int bn = b.Columns;
            double[] column = new double[mRows];
            for (int j = 0; j < bn; j++)
            {
                for (int k = 0; k < mRows; k++)
                {
                    column[k] = b.ValueAt(k, j);
                }
                for (int i = 0; i < mRows; i++)
                {
                    double s = 0;
                    for (int k = 0; k < mRows; k++)
                    {
                        s += mQ.ValueAt(k, i)*column[k];
                    }
                    b.ValueAt(i, j, s);
                }
            }

            // Solve R*X = Y;
            for (int k = mColumns - 1; k >= 0; k--)
            {
                for (int j = 0; j < bn; j++)
                {
                    b.ValueAt(k, j, b.ValueAt(k, j)/mR.ValueAt(k, k));
                }
                for (int i = 0; i < k; i++)
                {
                    for (int j = 0; j < bn; j++)
                    {
                        b.ValueAt(i, j, b.ValueAt(i, j) - b.ValueAt(k, j)*mR.ValueAt(i, k));
                    }
                }
            }
            for (int i = 0; i < mR.Columns; i++)
            {
                for (int j = 0; j < b.Columns; j++)
                {
                    result.ValueAt(i, j, b.ValueAt(i, j));
                }
            }
        }

        protected override void DoSolve(Vector b, Vector result)
        {
            // Compute Y = transpose(Q)*B
            double[] column = new double[mRows];
            for (int k = 0; k < mRows; k++)
            {
                column[k] = b[k];
            }
            for (int i = 0; i < mRows; i++)
            {
                double s = 0;
                for (int k = 0; k < mRows; k++)
                {
                    s += mQ.ValueAt(k, i)*column[k];
                }
                b[i] = s;
            }

            // Solve R*X = Y;
            for (int k = mColumns - 1; k >= 0; k--)
            {
                b[k] /= mR.ValueAt(k, k);
                for (int i = 0; i < k; i++)
                {
                    b[i] -= b[k]*mR.ValueAt(i, k);
                }
            }
            for (int i = 0; i < mR.Columns; i++)
            {
                result[i] = b[i];
            }
        }

        private static double[] GenerateColumn(Matrix A, int r1, int r2, int c)
        {
            int ru = r2 - r1 + 1;
            double[] u = new double[ru];

            for (int i = r1; i <= r2; i++)
            {
                u[i - r1] = A.ValueAt(i, r1);
                A.ValueAt(i, r1, 0.0);
            }

            double norm = Norm(u);

            if (r1 == r2 || norm == 0)
            {
                A.ValueAt(r1, c, -u[0]);
                u[0] = System.Math.Sqrt(2.0);
                return u;
            }

            double scale = 1.0/norm;
            if (u[0] < 0.0)
            {
                scale *= -1.0;
            }

            A.ValueAt(r1, c, -1.0/scale);

            for (int i = 0; i < ru; i++)
            {
                u[i] *= scale;
            }

            u[0] += 1.0;
            double s = System.Math.Sqrt(1.0/u[0]);

            for (int i = 0; i < ru; i++)
            {
                u[i] *= s;
            }
            return u;
        }

        private static void UA(double[] u, Matrix A, int r1, int r2, int c1, int c2, double[] v)
        {
            if (r2 < r1 || c2 < c1)
            {
                return;
            }

            for (int j = c1; j <= c2; j++)
            {
                v[j - c1] = 0.0;
            }

            for (int i = r1; i <= r2; i++)
            {
                for (int j = c1; j <= c2; j++)
                {
                    v[j - c1] = v[j - c1] + u[i - r1]*A.ValueAt(i, j);
                }
            }

            for (int i = r1; i <= r2; i++)
            {
                for (int j = c1; j <= c2; j++)
                {
                    A.ValueAt(i, j, A.ValueAt(i, j) - u[i - r1]*v[j - c1]);
                }
            }
        }

        private static void UA(double[] u, Matrix A, int r1, int r2, int c1, int c2)
        {
            if (c1 > c2)
            {
                return;
            }
            UA(u, A, r1, r2, c1, c2, new double[c2 - c1 + 1]);
        }
    }
}